#include "tp_type.h"
#include "../control.h"
#include "tc.h"
#include "blendmath.h"
#include "speed_ctrl.h"
#include "tcq.h"
#include "../kinematics/kinematics.h"
#include <string.h>
#include <iostream>
#include <sstream>

//============================
extern STRUCT_CONTROL Ct;

//=============================
int tpAddSegmentToQueue(STRUCT_TC *tc);
int tpCompleteSegment(STRUCT_TP *tp,STRUCT_TC *tc);
void tpEmptyQueue(STRUCT_TP *tp);
int tpHandleRegularCycle(STRUCT_TP *tp,STRUCT_TC *tc, STRUCT_TC *nexttc);

//===========================
int tpCheckSegmentError(STRUCT_TC *tc);
int tpActivateSegment(STRUCT_TC *tc, STRUCT_TC *nexttc);
int tpHandleSplitCycle(STRUCT_TC *tc, STRUCT_TC *nexttc);
int tpUpdateCycle(STRUCT_TP *tp, STRUCT_TC *tc, STRUCT_TC  *nexttc);
int tpCheckEndCondition(STRUCT_TC *tc, STRUCT_TC  *nexttc);
int tpComputeBlendVelocity(STRUCT_TP *tp, STRUCT_TC *tc, STRUCT_TC *nexttc);
int tpDoParabolicBlending(STRUCT_TP *tp, STRUCT_TC *tc, STRUCT_TC *nexttc);
void tpUpdateBlend(STRUCT_TP *tp, STRUCT_TC *tc, STRUCT_TC *nexttc);
int tpProcessAbortAndPause(STRUCT_TP *tp, STRUCT_TC *tc, STRUCT_TC *nexttc);
void tpProcessError(STRUCT_TP *tp, STRUCT_TC *tc, STRUCT_TC *nexttc);
void tpDoneAbort(STRUCT_TP *tp);
int tpUpdateState(STRUCT_TP *tp, STRUCT_TC *tc, STRUCT_TC *nexttc);
int tpCheckQueueFull(int id);

//get vel
float tpGetFeedRate();
float tpGetRealFinalVel(STRUCT_TP *tp, STRUCT_TC *tc, STRUCT_TC *nexttc);
float tpGetRealTargetVel(STRUCT_TP *tp, STRUCT_TC *tc, STRUCT_TC *nexttc);
float tpGetRealAccel(STRUCT_TP *tp, STRUCT_TC *tc, STRUCT_TC *nexttc);
float tpCalculateTriangleVel(STRUCT_TP *tp, STRUCT_TC *tc, STRUCT_TC *nexttc);

//blend
int tpHandleBlendArc(STRUCT_TP * tp, STRUCT_TC *tc, STRUCT_TC *prev_tc);
int tpSetupTangent(STRUCT_TP *tp, STRUCT_TC *prev_tc, STRUCT_TC *tc);
int tpCheckBlendArcType(STRUCT_TP *tp, STRUCT_TC *prev_tc, STRUCT_TC *tc);
int tpCreateLineLineBlend(STRUCT_TP * tp, STRUCT_TC * prev_tc, STRUCT_TC * tc, STRUCT_TC* blend_tc);

//=============================
/*tp creat funtion,it will creat a trajctory planner*/
int tpCreate(int _queueSize)
{
	if (_queueSize <= 0) {
		return -1;
	}
	else {
		Ct.tp.queue_size = _queueSize;
	}
	if (-1 == tcqCreate(&Ct.tp.queue, Ct.tp.queue_size, Ct.tc_queue_space)){
		return -1;
	}
	return 0;
}

/*clear tp module data*/
int tpClear(STRUCT_TP *tp)
{
	tcqInit(&tp->queue);
	memset(&tp->crp, 0, sizeof(double)*MAX_JOINT);
	memset(&tp->goal_posit, 0, sizeof(double)*MAX_JOINT);
	tp->tp_have_error = 0;
	//memset(&tp->goal_pose, 0, sizeof(STRUCT_POSE));
	return 0;
}

/**
* Calculate an updated goal position for the next timestep.
* This is the brains of the operation. It's called every TRAJ period and is
* expected to set tp->currentPos to the new machine position. Lots of other
* const tp fields (depth, done, etc) have to be twiddled to communicate the
* status; I think those are spelled out here correctly and I can't clean it up
* without breaking the API that the TP presents to motion.
*/
int tpRunCycle(STRUCT_TP *tp)
{
	STRUCT_TC *tc = 0;
	STRUCT_TC *nexttc = 0;
	int ret = 0;
	if (CMD_STATUS_PAUSED == Ct.cmd_status) return -1;
	if (1){		//blocking because of buff
		tc = tcqItem(&tp->queue, 0);
		nexttc = tcqItem(&tp->queue, 1);
		if (0 == tc) {
			tpEmptyQueue(tp);
			return -1;
		}
		if (0 == tc->vl.active){
			ret = tpActivateSegment(tc, nexttc);
			if (ret != 0) return -1;
		}
		if (tpCheckSegmentError(tc) != 0){
			tpProcessError(tp,tc,nexttc); //??? how to process error
			return -1;
		}
		if (tc->vl.split) {
			tpHandleSplitCycle(tc, nexttc);
			if (nexttc != 0)tpHandleRegularCycle(tp,nexttc, NULL);
		}
		else {
			tpHandleRegularCycle(tp,tc, nexttc);
		}
		//clear flag
		tpUpdateState(tp, tc, nexttc);
		if (tc->remove){
			tpCompleteSegment(tp,tc);
		}
		return 0;
	}
	return -1;
}

/*updata state*/
int tpUpdateState(STRUCT_TP *tp, STRUCT_TC *tc, STRUCT_TC *nexttc)
{
	tpProcessAbortAndPause(tp, tc, nexttc);
	return 0;
}

int tpCompleteSegment(STRUCT_TP *tp, STRUCT_TC *tc)
{
	int ret = 0;
	tcqRemove(&tp->queue, 1);
	std::ostringstream ostr;
	ostr << "finish tc id =" << tc->id;
	std::string sstr = ostr.str();
	Ct.conf.callback_msg(MOTION_MSG_FINISH_SEGMENT,(char *)sstr.c_str());
	DLog("Tp->***********Finished tc id %d*************\n", tc->id);
	return 0;
}


//************************************
// Method:    tpEmptyQueue
// FullName:  tpEmptyQueue
// Access:    1->clear cmd_status,2->initial tcp,3->set goal_posit
// Returns:   void
// Qualifier:
// Parameter: STRUCT_TP * tp
//************************************
void tpEmptyQueue(STRUCT_TP *tp)
{
	if (1 == Ct.running){
		Ct.running = 0;
		Ct.cmd_status = CMD_STATUS_MOVING_IDLE;
		tcqInit(&tp->queue);
		memcpy(tp->goal_posit, tp->crp, sizeof(double)*MAX_JOINT);
		Ct.execute_id = -1;
	}
	return;
}

//a regular cycle
int tpHandleRegularCycle(STRUCT_TP *tp,STRUCT_TC *tc, STRUCT_TC *nexttc)
{
	int ret = 0;
	if (tc->remove) return 0;
	tpUpdateCycle(tp,tc, nexttc);
	tpComputeBlendVelocity(tp, tc, nexttc);
	if (nexttc && tcIsBlending(tc)){
		//DLog("Doing Blending\n");
		tpDoParabolicBlending(tp, tc, nexttc);
	}
	return 0;
}

//get vel and limit vel
float tpGetFeedRate()
{
	if (Ct.pausing) {
		return 0.0;
	}
	else if (Ct.aborting){
		return 0.0;
	}
	else {
		return Ct.feed_rate;
	}
}

//************************************
// Method:    tpGetRealFinalVel
// FullName:  tpGetRealFinalVel
// Access:    get the final velocity
// Returns:   float
// Qualifier:
// Parameter: STRUCT_TP * tp
// Parameter: STRUCT_TC * tc
// Parameter: STRUCT_TC * nexttc
//************************************
float tpGetRealFinalVel(STRUCT_TP *tp, STRUCT_TC *tc, STRUCT_TC *nexttc)
{
	float target_vel = tpGetRealTargetVel(tp, tc, nexttc);
	tc->vl.real_final_vel = Bl_MIN(tc->vl.finalvel,target_vel);
	return tc->vl.real_final_vel;
}
float tpGetRealTargetVel(STRUCT_TP *tp, STRUCT_TC *tc, STRUCT_TC *nexttc)
{
	tc->vl.real_target_vel = tc->vl.req_max_vel*tpGetFeedRate();
	return tc->vl.real_target_vel;
}
float tpGetRealAccel(STRUCT_TP *tp, STRUCT_TC *tc, STRUCT_TC *nexttc)
{
	tc->vl.real_accel = tc->vl.req_acca;
	return tc->vl.real_accel;
}
float tpCalculateTriangleVel(STRUCT_TP *tp, STRUCT_TC *tc, STRUCT_TC *nexttc)
{
	float acc_scaled = tpGetRealAccel(tp,tc,nexttc);
	float length = (float)tc->vl.target;
	float triangle_vel = blSqrt(acc_scaled * length);
	return triangle_vel;
}

//update cycle
int tpUpdateCycle(STRUCT_TP *tp,STRUCT_TC *tc, STRUCT_TC  *nexttc)
{
	int ret = 0;
	float vel = 0;
	double p2p_before[MAX_JOINT], p2p_dist[MAX_JOINT];
	STRUCT_POSE pose_befor, pose_dist, pose_curr;
	static STRUCT_POSE last_pose;
	if (tc->patten == EM_TC_P2P){
		tcP2PGetPosit(tc, p2p_before);
	}
	else if (tc->patten == EM_TC_LINE){
		tcGetLine9Pose(&tc->move.line9,&pose_befor,tc->vl.progress,tc->vl.target);
	}
	STRUCT_SPEED_CTRL *vl = &tc->vl;
	tpGetRealFinalVel(tp, tc, nexttc);
	tpGetRealTargetVel(tp, tc, nexttc);
	tpGetRealAccel(tp, tc, nexttc);
	spCalculateTrapezoidalAccel(vl, &vel);
	spUpdateDistFromAccel(vl, vel);
	tpCheckEndCondition(tc, nexttc);
	if (tc->patten == EM_TC_P2P){
		tcP2PGetPosit(tc, p2p_dist);
		tcP2PSelfSub(p2p_dist, p2p_before);
		tcP2PSelfAdd(tp->crp, p2p_dist);
	}
	else if (tc->patten == EM_TC_LINE){
		tcGetLine9Pose(&tc->move.line9, &pose_dist, tc->vl.progress, tc->vl.target);
		tcLine9SelfSub(&pose_dist, &pose_befor);
		Kinematic_ClampForward(tp->crp, &pose_curr, NULL);
		if(fabs(last_pose.x - pose_curr.x) > 1e-3 ||
			fabs(last_pose.y - pose_curr.y) > 1e-3 ||
			fabs(last_pose.z - pose_curr.z) > 1e-3){
			//DError("check line last pose and current pose faild\n");
			Kinematic_ClampForward(tp->crp, &pose_curr, NULL);
			//Ct.conf.callback_msg(9999, NULL);
		}
		tcLine9SelfAdd(&pose_curr, &pose_dist);
		last_pose = pose_curr;
		ret = Kinematic_ClampInverse(p2p_dist, &pose_curr, tc->move.line9.ik_flag, NULL);
		if (ret == 0){
			memcpy(tp->crp, p2p_dist, sizeof(double)*MAX_JOINT);
		}
	}
	return 0;
}

/*check a segement is fualt*/
int tpCheckSegmentError(STRUCT_TC *tc)
{
	if (0 == tc->tc_error_flag) return 0;
	else{
		if (tc->tc_error_flag & TC_ERROR_P2P_POSE_E){
			DError("Tp->TC_ERROR_P2P_POSE_E,id=%d", tc->id);
			Controller_MsgVp(MOTION_WARN_POSE_ERROR, "id=%d", tc->id);
		}
		if (tc->tc_error_flag & TC_ERROR_USER_VEL) {
			DError("Tp->TC_ERROR_USER_VEL_ACCA,id=%d", tc->id);
			Controller_MsgVp(MOTION_WARN_ERROR_USER_VEL, "id=%d", tc->id);
		}
		if (tc->tc_error_flag & TC_ERROR_USER_OVL){
			DError("Tp->TC_ERROR_USER_OVL,id=%d", tc->id);
			Controller_MsgVp(MOTION_WARN_ERROR_USER_OVL, "id=%d", tc->id);
		}
		if (tc->tc_error_flag & TC_ERROR_LINE_POSE_S){
			DError("Tp->TC_ERROR_LINE_POSE_S,id=%d", tc->id);
			Controller_MsgVp(MOTION_WARN_POSE_ERROR, "id=%d", tc->id);
		}
		if (tc->tc_error_flag & TC_ERROR_LINE_POSE_E){
			DError("Tp->TC_ERROR_LINE_POSE_E,id=%d", tc->id);
			Controller_MsgVp(MOTION_WARN_POSE_ERROR, "id=%d", tc->id);
		}
		if (tc->tc_error_flag & TC_ERROR_LINE_FK_FLAG_UNEUQAL_IK_FLAG){
			DError("Tp->TC_ERROR_LINE_FK_FLAG_UNEUQAL_IK_FLAG,id=%d", tc->id);
			Controller_MsgVp(MOTION_WARN_FK_FLAG_UNEQUAL_IK_FLAG, "id=%d", tc->id);
		}
		return -1;
	}
}

//active a segment
int tpActivateSegment(STRUCT_TC *tc, STRUCT_TC *nexttc)
{
	int ret = 0;
	if (!tc || tc->vl.active) {
		return 0;
	}
	else{
		tcActiveSegment(tc);
		Ct.execute_id = tc->id;
	}
	return 0;
}

//tcsplit
int tpHandleSplitCycle(STRUCT_TC *tc, STRUCT_TC *nexttc)
{
	tc->remove = 1;
	return 0;
}

//Check end
int tpCheckEndCondition(STRUCT_TC *tc, STRUCT_TC  *nexttc)
{
	float dx = (float)tc->vl.target - (float)tc->vl.progress;
	if (dx < TP_POSIT_EPSILON){
		if (!Ct.aborting && !Ct.pausing) tc->vl.progress = tc->vl.target;
		tcSetSplitCycle(tc, 0.0, tc->vl.current_vel);
		tpHandleSplitCycle(tc, nexttc);
		return 0;
	}
	if (tc->vl.on_final_decel && tc->vl.current_vel <= 0.0){
		if (!Ct.aborting && !Ct.pausing) tc->vl.progress = tc->vl.target;
		tcSetSplitCycle(tc, 0.0, tc->vl.current_vel);
		tpHandleSplitCycle(tc, nexttc);
		return 0;
	}
	float temp_dx = tc->vl.current_vel*Ct.conf.inter_period*0.5;
	if (dx < temp_dx){
		if (!Ct.aborting && !Ct.pausing) tc->vl.progress = tc->vl.target;
		tcSetSplitCycle(tc, 0.0, tc->vl.current_vel);
		tpHandleSplitCycle(tc, nexttc);
		DLog("splite while dx too small,dx=%f,v_dx=%f", dx, temp_dx);
		return 0;
	}
	return 0;
}

//calculate blend velocity
int tpComputeBlendVelocity(STRUCT_TP *tp,STRUCT_TC *tc, STRUCT_TC *nexttc)
{
	if (!tc || !nexttc) {
		return 0;
	}
	if (nexttc->tc_error_flag != 0 || nexttc->tc_error_flag != 0 
		|| nexttc->user_ovl.rel < TP_USEROVL_ABS_EPSILON){
		tc->blend_vel = 0;
		return 0;
	}
	if (tc->patten == EM_TC_P2P && nexttc->patten != EM_TC_P2P){
		tc->blend_vel = 0;
		return 0;
	}
	if (tc->patten != EM_TC_P2P && nexttc->patten == EM_TC_P2P){
		tc->blend_vel = 0;
		return 0;
	}
	float acc_this = tpGetRealAccel(tp, tc, nexttc);
	float acc_next = tpGetRealAccel(tp,nexttc,NULL);
	float target_vel_this = tpGetRealTargetVel(tp, tc, nexttc);
	float target_vel_next = tpGetRealTargetVel(tp, nexttc, NULL);
	float v_reachable_this = Bl_MIN(tpCalculateTriangleVel(tp, tc, nexttc), target_vel_this);
	float v_reachable_next = Bl_MIN(tpCalculateTriangleVel(tp, nexttc, NULL), target_vel_next);
	if (v_reachable_next < TP_VEL_EPSILON) return 0;
	float t_max_this = (float)tc->vl.target / (float)v_reachable_this;
	float t_max_next = (float)nexttc->vl.target / (float)v_reachable_next;
	float t_max_reachable = Bl_MIN(t_max_this, t_max_next);
	float t_min_blend_this = v_reachable_this / acc_this;
	float t_min_blend_next = v_reachable_next / acc_next;
	float t_max_blend = Bl_MAX(t_min_blend_this, t_min_blend_next);
	float t_blend = fmin(t_max_reachable, t_max_blend);
	float v_blend_this = Bl_MIN(v_reachable_this, t_blend * acc_this);
	float v_blend_next = Bl_MIN(v_reachable_next, t_blend * acc_next);
	tc->blend_vel = v_blend_this*tc->user_ovl.rel;
	nexttc->blend_vel = v_blend_next*nexttc->user_ovl.rel;
	return 0;
}

//do parabolic blending
int tpDoParabolicBlending(STRUCT_TP *tp, STRUCT_TC *tc, STRUCT_TC *nexttc)
{
	//DLog("Tp->in DoParabolicBlend\n");
	//DLog("tbc=%f,tcv=%f,on_f=%d,tbn=%f\n", tc->blend_vel, tc->vl.current_vel, tc->vl.on_final_decel, nexttc->blend_vel);
	tpUpdateBlend(tp, tc, nexttc);
	return 0;
}

//update blend 
void tpUpdateBlend(STRUCT_TP *tp, STRUCT_TC *tc, STRUCT_TC *nexttc)
{
	int ret = 0;
	tpUpdateCycle(tp, nexttc, NULL);
	return;
}

/*done abort commond it will clear empty_queue*/
void tpDoneAbort(STRUCT_TP *tp)
{
	Ct.aborting = 0;
	Ct.pausing = 0;
	tpEmptyQueue(tp);		//tp done abort have to empty queue
	return;
}

void tpProcessError(STRUCT_TP *tp, STRUCT_TC *tc, STRUCT_TC *nexttc)
{
	tp->tp_have_error = 1;
	tpDoneAbort(tp);
	DLog("Tp->Done segment error process");
	return;
}

//process abort or pause
int tpProcessAbortAndPause(STRUCT_TP *tp, STRUCT_TC *tc, STRUCT_TC *nexttc)
{
	int i = 0;
	if (Ct.aborting){
		if ((tc->vl.current_vel <= 0.0 || tc->remove) && (!nexttc || nexttc->vl.current_vel <= 0.0)){
			tpDoneAbort(tp);
			DLog("Tp->Done Command->ABORT");
			return 0;
		}
	}
	if (Ct.pausing){
		if ((tc->vl.current_vel <= 0.0 || tc->remove) && (!nexttc || nexttc->vl.current_vel <= 0.0)){
			Ct.pausing = 0;
			Ct.cmd_status = CMD_STATUS_PAUSED;
			DLog("Tp->Done Command->PAUSE");
			return 0;
		}
	}
	return 0;
}

/*add a point to point movement*/
int tpAddP2P(double posit[MAX_JOINT], STRUCT_USER_VEL user_vel, STRUCT_USER_OVL user_ovl, int id, char enable)
{
	DLog("Tp->---------Start Add P2P Theta---------");
	STRUCT_TC tc = { 0 };
	int ret = 0;
	tc.patten = EM_TC_P2P;
	tcInitSegment(&tc, user_vel, user_ovl, id, enable);
	if (tpCheckQueueFull(id) != 0) return -1;
	tcCheckUserVelAndOvl(&tc);
	memcpy(tc.move.p2p.pe,posit,sizeof(double)*MAX_JOINT);
	memcpy(tc.move.p2p.ps, Ct.tp.goal_posit, sizeof(double)*MAX_JOINT);
	ret = Kinematic_ClampForward(posit, &tc.move.p2p.pose_end,&tc.move.p2p.end_fk_flag);
	if (ret != 0) {
		tc.move.p2p.pose_end_error = 1;
		tc.tc_error_flag |= TC_ERROR_P2P_POSE_E;
		DError("forward kinematic fiald, id = %d", id);
		goto FEND;
	}
	else tc.move.p2p.pose_end_error = 0;
	ret = tcPreProcessSegment_P2P(&tc);
	if (ret != 0){
		DWarn("p2p target is less than epsilon, skip this segment, id = %d", id);
		return 0;
	}
FEND:
	tpAddSegmentToQueue(&tc);
	memcpy(Ct.tp.goal_posit, tc.move.p2p.pe, sizeof(double)*MAX_JOINT);
	DLog("Tp->Add P2P Movement done,Id=%d, Enable=%d, error_flag=%d\n", id,enable, tc.tc_error_flag);
	return 0;
}

/*add a point to point movement*/
int tpAddP2P_Pose(STRUCT_POSE pose, int ik_flag, STRUCT_USER_VEL user_vel, STRUCT_USER_OVL user_ovl, int id, char enable)
{
	DLog("Tp->---------Start Add P2P Pose---------");
	STRUCT_TC tc = { 0 };
	int ret = 0;
	tc.patten = EM_TC_P2P;
	double posit[MAX_JOINT];
	tcInitSegment(&tc, user_vel, user_ovl, id, enable);
	if (tpCheckQueueFull(id) != 0) return -1;
	tcCheckUserVelAndOvl(&tc);
	ret = Kinematic_ClampInverse(posit, &pose, ik_flag, NULL);
	if (ret != 0) {
		tc.move.p2p.pose_end_error = 1;
		tc.tc_error_flag |= TC_ERROR_P2P_POSE_E;
		DError("inverse kinematic fiald, id = %d", id);
		goto FEND;	//goto function end,tc have error and  don;t need process the segment
	}
	else tc.move.p2p.pose_end_error = 0;
	memcpy(tc.move.p2p.pe, posit, sizeof(double)*MAX_JOINT);
	memcpy(tc.move.p2p.ps, Ct.tp.goal_posit, sizeof(double)*MAX_JOINT);
	ret = tcPreProcessSegment_P2P(&tc);
	if (ret != 0){
		DWarn("p2p target is less than epsilon,skip this segment,id = %d", id);
		return 0;
	}
FEND:
	tpAddSegmentToQueue(&tc);
	memcpy(Ct.tp.goal_posit, tc.move.p2p.pe, sizeof(double)*MAX_JOINT);
	DLog("Tp->Add P2P Movement done,Id=%d, Enable=%d, error_flag=%d\n", id, enable, tc.tc_error_flag);
	return 0;
}


/*add a segment to queue*/
int tpAddSegmentToQueue(STRUCT_TC *tc)
{
	tcqPut(&Ct.tp.queue, tc);
	Ct.running = 1;
	Ct.cmd_status = CMD_STATUS_MOVING_PLANE;
	return 0;
}

//check queue
int tpCheckQueueFull(int id)
{
	//if queue length more than TCQ_QUEUE_LEN_ALL_FULL
	//wo maintaind that buff is full
	if (Ct.tp.queue._len > TCQ_QUEUE_LEN_ALL_FULL){
		DLog("Tp->buff already full,write cmd fiald,id=%d\n",id);
		return -1;
	}
	return 0;
}

/*Control Handle abort*/
int tpHandleAbort(STRUCT_TP *tp)
{
	DLog("Tp->Progressing Command->ABORT");
	tp->tp_have_error = 1;
	if (CMD_STATUS_PAUSED == Ct.cmd_status){
		tpDoneAbort(tp);
		DLog("Tp->Done Command->ABORT,Last CmdStatus is PAUSED");
		return 0;
	}
	Ct.cmd_status = CMD_STATUS_ABORTING;
	Ct.aborting = 1;
	Ct.pausing = 0;
	return 0;
}

int tpHandlePause(STRUCT_TP *tp)
{
	DLog("Tp->Progressing Command->PAUSE");
	Ct.cmd_status = CMD_STATUS_PAUSING;
	Ct.pausing = 1;
	return 0;
}

int tpHandleResume(STRUCT_TP *tp)
{
	DLog("Tp->Done Command->RESUME");
	Ct.cmd_status = CMD_STATUS_MOVING_PLANE;
	Ct.pausing = 0;
	return 0;
}

//************************************
// Method:    tpHandleChangeVelRate
// FullName:  tpHandleChangeVelRate
// Access:    �ı������
// Returns:   int
// Qualifier:
// Parameter: STRUCT_TP * tp
// Parameter: float feed_rate
//************************************
int tpHandleChangeVelRate(STRUCT_TP *tp, float feed_rate)
{
	DLog("Tp->Done Command->ChangeVel,Vel=%f", feed_rate);
	Ct.feed_rate = feed_rate;
	return 0;
}


//************************************
// Method:    tpAddLine
// FullName:  tpAddLine
// Access:    public 
// Returns:   int
// Qualifier:
// Parameter: STRUCT_POSE pose
// Parameter: int ik_flag
// Parameter: STRUCT_USER_VEL user_vel
// Parameter: STRUCT_USER_OVL user_ovl
// Parameter: int id
// Parameter: char enable
//************************************
int tpAddLine(STRUCT_POSE pose, int ik_flag, STRUCT_USER_VEL user_vel, STRUCT_USER_OVL user_ovl, int id, char enable)
{
	DLog("Tp->---------Start Add Line---------");
	STRUCT_TC tc = { 0 };
	STRUCT_TC *prev_tc;
	int ret = 0;
	tc.patten = EM_TC_LINE;
	double inverse_posit[MAX_JOINT];
	STRUCT_POSE start_pose;
	int fk_flag;
	ret = Kinematic_ClampForward(Ct.tp.goal_posit, &start_pose, &fk_flag);
	if (ret != 0) {
		tc.tc_error_flag |= TC_ERROR_LINE_POSE_E;
		DError("forward kinematic fiald, id = %d", id);
		goto FEND;
	}
	if (fk_flag != ik_flag && fk_flag != KINE_FK_SINGULAR){
		tc.tc_error_flag |= TC_ERROR_LINE_FK_FLAG_UNEUQAL_IK_FLAG;
		DError("forward kinematic flag is not equal with inverse kinematic flag, id = %d", id);
		goto FEND;
	}
	tcInitSegment(&tc, user_vel, user_ovl, id, enable);
	if (tpCheckQueueFull(id) != 0) return -1;
	tcCheckUserVelAndOvl(&tc);
	ret = Kinematic_ClampInverse(inverse_posit, &pose, ik_flag, NULL);
	if (ret != 0) {
		tc.tc_error_flag |= TC_ERROR_LINE_POSE_E;
		DError("inverse kinematic fiald, id = %d", id);
	}
	spClear(&tc.vl);
	tcLine9Init(&tc.move.line9, &start_pose, &pose, ik_flag);
	ret = tcGetLine9TargetLen(&tc.move.line9, &tc.vl.target);
	if (ret != 0){
		//target is less than epsilon,ignore this segment movement
		DWarn("line target is less than epsilon,id = %d",id);
		return 0;
	}
	if (tc.move.line9.xyz.tmag_zero && tc.move.line9.rxyz.tmag_zero){
		//only have uvw movements,change movement to p2p
		DWarn("change move line to p2p,for line and rot target is zero,id = %d", id);
		ret = tpAddP2P_Pose(pose, ik_flag, user_vel, user_ovl, id, enable);
		return ret;
	}
	ret = tcPreProcessSegment_Line(&tc);
	if(ret != 0) return -1;
#if 0
	prev_tc = tcqLast(&Ct.tp.queue);
	if (tc.move.line9.xyz.tmag_zero || prev_tc == NULL || prev_tc->patten == EM_TC_P2P){
		//this line only have pose rot movement,ro don't have prev_tc move
		//or prev_tc is a p2p movement
		//stop look-ahead
		tpAddSegmentToQueue(&tc);
		memcpy(Ct.tp.goal_posit, inverse_posit, sizeof(double)*MAX_JOINT);
		DLog("Tp->Add Line Movement done,orot=%d,Id=%d, Enable=%d, error_flag=%d\n", tc.move.line9.xyz.tmag_zero, id, enable, tc.tc_error_flag);
		return 0;
	}
	ret = tpHandleBlendArc(&Ct.tp, &tc, prev_tc);
	if (ret != 0){
		DLog("-----------Skip Arc Blending------------");
	}
#endif
	tpAddSegmentToQueue(&tc);
	memcpy(Ct.tp.goal_posit, inverse_posit, sizeof(double)*MAX_JOINT);
	DLog("Tp->Add Line Movement done,Id=%d, Enable=%d, error_flag=%d\n", id, enable, tc.tc_error_flag);
FEND:
	return 0;
}


//************************************
// Method:    tpCreateLineLineBlend
// FullName:  tpCreateLineLineBlend
// Access:    public 
// Returns:   int
// Qualifier:
// Parameter: STRUCT_TP * tp
// Parameter: STRUCT_TC * prev_tc
// Parameter: STRUCT_TC * tc
// Parameter: STRUCT_TC * blend_tc
//************************************
int tpCreateLineLineBlend(STRUCT_TP * tp, STRUCT_TC * prev_tc, STRUCT_TC * tc, STRUCT_TC* blend_tc)
{
	AbLog("-- Starting LineLine blend arc --\n");
	STRUCT_PM_CARTESIAN acc_bound, vel_bound;

	//Get machine limits
	acc_bound.x = Ct.conf.vel_line.acc_max;
	acc_bound.y = Ct.conf.vel_line.acc_max;
	acc_bound.z = Ct.conf.vel_line.acc_max;
	vel_bound.x = Ct.conf.vel_line.vel_max;
	vel_bound.y = Ct.conf.vel_line.vel_max;
	vel_bound.z = Ct.conf.vel_line.vel_max;
	// Setup blend data structures
#if 0
	STRUCT_BLEND_GOEM geom;
	BlendParameters param;
	BlendPoints3 points;

	int res_init = blendInit3FromLineLine(&geom, &param,
		prev_tc,
		tc,
		&acc_bound,
		&vel_bound,
		emcmotConfig->maxFeedScale);

	if (res_init != TP_ERR_OK) {
		tp_debug_print("blend init failed with code %d, aborting blend arc\n",
			res_init);
		return res_init;
	}

	int res_blend = blendComputeParameters(&param);
	if (res_blend != TP_ERR_OK) {
		return res_blend;
	}

	blendFindPoints3(&points, &geom, &param);

	blendCheckConsume(&param, &points, prev_tc, emcmotConfig->arcBlendGapCycles);

	// Set up actual blend arc here
	int res_arc = arcFromBlendPoints3(&blend_tc->coords.arc.xyz, &points, &geom, &param);
	if (res_arc < 0) {
		return TP_ERR_FAIL;
	}

	// Note that previous restrictions don't allow ABC or UVW movement, so the
	// end and start points should be identical
	blend_tc->coords.arc.abc = prev_tc->coords.line.abc.end;
	blend_tc->coords.arc.uvw = prev_tc->coords.line.uvw.end;

	//set the max velocity to v_plan, since we'll violate constraints otherwise.
	tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req,
		param.v_plan, param.a_max);
	blend_tc->target_vel = param.v_actual;

	if (tpCheckTangentPerformance(tp, prev_tc, tc, blend_tc) == TP_ERR_NO_ACTION) {
		return TP_ERR_NO_ACTION;
	}

	int retval = TP_ERR_FAIL;

	//TODO refactor to pass consume to connect function
	if (param.consume) {
		//Since we're consuming the previous segment, pop the last line off of the queue
		retval = tcqPopBack(&tp->queue);
		if (retval) {
			//This is unrecoverable since we've already changed the line. Something is wrong if we get here...
			//rtapi_print_msg(RTAPI_MSG_ERR, "PopBack failed\n");
			return TP_ERR_FAIL;
		}
		//Since the blend arc meets the end of the previous line, we only need
		//to "connect" to the next line
		retval = tcConnectBlendArc(NULL, tc, &points.arc_start, &points.arc_end);
	}
	else {
		//TODO refactor connect function to stretch lines and check for bad stretching
		tp_debug_print("keeping previous line\n");
		retval = tcConnectBlendArc(prev_tc, tc, &points.arc_start, &points.arc_end);
	}
	return retval;
#endif
	return 0;
}

//************************************
// Method:    tpHandleBlendArc
// FullName:  tpHandleBlendArc
// Access:    public 
// Returns:   int
// Qualifier:
// Parameter: STRUCT_TP * tp
// Parameter: STRUCT_TC * tc
// Parameter: STRUCT_TC * prev_tc
//************************************
int tpHandleBlendArc(STRUCT_TP * tp, STRUCT_TC *tc, STRUCT_TC *prev_tc)
{
	AbLog("============Handle Blend Arc=============");
	if (prev_tc->vl.progress > prev_tc->vl.target * 0.5) {
		return 0;
	}
	// Check for tangency between segments and handle any errors
	// TODO possibly refactor this into a macro?
	int res_tan = tpSetupTangent(tp, prev_tc, tc);
	if (res_tan != 0) return -1;

	STRUCT_TC blend_tc = { 0 };
#if 1
	int type = tpCheckBlendArcType(tp, prev_tc, tc);
	int res_create;
	switch (type) {
	case 1:
		res_create = tpCreateLineLineBlend(tp, prev_tc, tc, &blend_tc);
		break;
	case 2:
		//res_create = tpCreateLineArcBlend(tp, prev_tc, tc, &blend_tc);
		return -1;
		break;
	case 3:
		//res_create = tpCreateArcLineBlend(tp, prev_tc, tc, &blend_tc);
		return -1;
		break;
	case 4:
		//res_create = tpCreateArcArcBlend(tp, prev_tc, tc, &blend_tc);
		return -1;
		break;
	default:
		return -1;
		break;
	}

	if (res_create == 0) {
		//Need to do this here since the length changed
		tpAddSegmentToQueue(&blend_tc);
	}
	else {
		return res_create;
	}
	return 0;
#endif
}

/**
* Check for tangency between the current segment and previous segment.
* If the current and previous segment are tangent, then flag the previous
* segment as tangent, and limit the current segment's velocity by the sampling
* rate.
*/
int tpSetupTangent(STRUCT_TP *tp, STRUCT_TC *prev_tc, STRUCT_TC *tc) 
{
	if (!tc || !prev_tc) {
		AbLog("missing tc or prev tc in tangent check\n");
		return -1;
	}
#if 0 
	//arc blend allow rotary and uvw movement,2017-04-05-zqs
	//If we have ABCUVW movement, then don't check for tangency
	if (tpRotaryMotionCheck(tp, tc) || tpRotaryMotionCheck(tp, prev_tc)) {
		tp_debug_print("found rotary axis motion\n");
		return TP_ERR_FAIL;
	}
#endif
	if (Ct.conf.look_ahead_depth < 2) {
		//if look_ahead_depth is less than 2,don't need look ahead
		AbLog("Optimization depth %d too low for tangent optimization", Ct.conf.look_ahead_depth);
		return -1;
	}
	STRUCT_PM_CARTESIAN prev_tan, this_tan;
	int res_endtan = tcGetEndTangentUnitVector(prev_tc, &prev_tan);
	int res_starttan = tcGetStartTangentUnitVector(tc, &this_tan);
	AbLog("prev tangent vector: %f %f %f\n", prev_tan.x, prev_tan.y, prev_tan.z);
	AbLog("this tangent vector: %f %f %f\n", this_tan.x, this_tan.y, this_tan.z);
	double dot = -1.0;
	const double SHARP_CORNER_THRESHOLD = cos(PM_PI * (1.0 - SHARP_CORNER_DEG / 180.0));
	tcCartCartDot(&prev_tan, &this_tan, &dot);
	if (dot < SHARP_CORNER_THRESHOLD) {
		//arc blend rad,if start and end vector > SHARP_CORNER_DEG stop arc blend
		AbLog("Found sharp corner\n");
		return -1;
	}

	// Calculate instantaneous acceleration required for change in direction
	// from v1 to v2, assuming constant speed
	double v_max1 = prev_tc->vl.req_max_vel;
	double v_max2 = tc->vl.req_max_vel;
	double v_max = fmin(v_max1, v_max2);
	AbLog("tangent v_max = %f\n", v_max);

	double a_inst = v_max / Ct.conf.inter_period;
	// Set up worst-case final velocity,if there only one cycle
	STRUCT_PM_CARTESIAN acc1, acc2, acc_diff;
	tcCartScalMult(&prev_tan, a_inst, &acc1);
	tcCartScalMult(&this_tan, a_inst, &acc2);
	tcCartCartSub(&acc2, &acc1, &acc_diff);

	//TODO store this in TP struct instead?
	STRUCT_PM_CARTESIAN acc_bound;
	acc_bound.x = Ct.conf.vel_line.acc_max;
	acc_bound.y = Ct.conf.vel_line.acc_max;
	acc_bound.z = Ct.conf.vel_line.acc_max;
	STRUCT_PM_CARTESIAN acc_scale;
	if (acc_diff.x != 0)acc_scale.x = fabs(acc_diff.x / acc_bound.x);
	else acc_scale.y = 0;
	if (acc_diff.y != 0)acc_scale.y = fabs(acc_diff.y / acc_bound.y);
	else acc_scale.x = 0;
	if (acc_diff.z != 0)acc_scale.z = fabs(acc_diff.z / acc_bound.z);
	else acc_scale.z = 0;
#if 0
	//FIXME this ratio is arbitrary, should be more easily tunable
	double acc_scale_max = pmCartAbsMax(&acc_scale);
	//KLUDGE lumping a few calculations together here
	if (prev_tc->motion_type == TC_CIRCULAR || tc->motion_type == TC_CIRCULAR) {
		acc_scale_max /= BLEND_ACC_RATIO_TANGENTIAL;
	}

	const double kink_ratio = tpGetTangentKinkRatio();
	if (acc_scale_max < kink_ratio) {
		tp_debug_print(" Kink acceleration within %g, treating as tangent\n", kink_ratio);
		tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT);
		tc->kink_vel = v_max;
		tpAdjustAccelForTangent(tp, tc, acc_scale_max);
		tpAdjustAccelForTangent(tp, prev_tc, acc_scale_max);

		return TP_ERR_OK;
	}
	else {
		tc->kink_vel = v_max * kink_ratio / acc_scale_max;
		tp_debug_print("Kink acceleration scale %f above %f, kink vel = %f, blend arc may be faster\n",
			acc_scale_max,
			kink_ratio,
			tc->kink_vel);
		return TP_ERR_NO_ACTION;
	}
#else
	tc->kink_vel = tc->vl.req_max_vel;
#endif
	return 0;
}

//************************************
// Method:    tpCheckBlendArcType
// FullName:  tpCheckBlendArcType
// Access:    public 
// Returns:   int 0->NULL 1->LL 2->LC 3->CL 4->CC (L is Line,C is Cycle)
// Qualifier:
// Parameter: STRUCT_TP * tp
// Parameter: STRUCT_TC * prev_tc
// Parameter: STRUCT_TC * tc
//************************************
int tpCheckBlendArcType(STRUCT_TP *tp, STRUCT_TC *prev_tc, STRUCT_TC *tc)
{
	if (prev_tc->patten == EM_TC_P2P) return 0;
	else if (prev_tc->patten == EM_TC_LINE && prev_tc->patten == EM_TC_LINE) return 1;
	else if (prev_tc->patten == EM_TC_LINE && prev_tc->patten == EM_TC_CIR) return 2;
	else if (prev_tc->patten == EM_TC_CIR && prev_tc->patten == EM_TC_LINE) return 3;
	else if (prev_tc->patten == EM_TC_CIR && prev_tc->patten == EM_TC_CIR) return 4;
	return 0;
}

